from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    static_tf_BaseFootprint2base_link = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['--x','0','--y', '0','--z', '0.75',
                   '--yaw', '0','--pitch', '0', '--roll', '0',
                   '--frame-id','base_footprint','--child-frame-id', 'base_link'],
        output='screen'
    )
    static_tf_BaseLink2laser = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['--x','0.4','--y', '0','--z', '0.75',
                   '--yaw', '0','--pitch', '0', '--roll', '0',
                   '--frame-id','base_link','--child-frame-id', 'laser'],
        output='screen'
    )
    # ugv_base launch
    ugv_base_launch_path = os.path.join(
        get_package_share_directory('sls_launch'),
        'launch',
        'ugv_base.launch.py'
    )
    ugv_base_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(ugv_base_launch_path),
        launch_arguments={
            "RTK_serial_port":"/dev/ttyUSB0"
        }.items()
    )

    # rplidar_ros launch
    rplidar_launch_dir = get_package_share_directory('rplidar_ros')
    rplidar_launch_file = os.path.join(rplidar_launch_dir, 'launch', 'rplidar_s2_launch.py')
    rplidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(rplidar_launch_file),
        launch_arguments={
            "serial_port":"/dev/ttyUSB1"
        }.items()
    )
    # slam_toolbox launch
    slam_toolbox_launch_path = os.path.join(
        get_package_share_directory('slam_toolbox'),
        'launch',
        'online_async_launch.py'
    )
    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slam_toolbox_launch_path),
        launch_arguments={
            'use_sim_time': 'false',
        }.items()
    )


    return LaunchDescription([
        static_tf_BaseFootprint2base_link,
        static_tf_BaseLink2laser,
        rplidar_launch,
        slam_toolbox_launch,
        ugv_base_launch
    ])